clear;
measurement_matric=load('measurement_matrix.txt');
minus_matric=repmat(mean(measurement_matric,2),1,size(measurement_matric,2));
measurement_matric_center=measurement_matric-minus_matric;
[u,w,v]=svd(measurement_matric_center);
u3=u(:,1:3);
v3=v(:,1:3);
w3=w(1:3,1:3);
M=u3*sqrt(w3);
S=sqrt(w3)*v3';
figure; axis equal;  hold on; 
ST=S';
plot3(-ST(:,1), -ST(:,2), -ST(:,3), '*r');
grid on; xlabel('x'); ylabel('y'); zlabel('z'); axis equal;
estimated_points=M*S;
estimated_points_origin=estimated_points+minus_matric;
frame1=imread('C:/Users/vivienfanghua/Desktop/HW3/frame/frame00000001.jpg');
frame51=imread('C:/Users/vivienfanghua/Desktop/HW3/frame/frame00000051.jpg');
frame101=imread('C:/Users/vivienfanghua/Desktop/HW3/frame/frame00000101.jpg');
figure;imshow(frame1);hold on; title 'frame1';
plot(estimated_points_origin(1,:),estimated_points_origin(2,:),'.r');
plot(measurement_matric(1,:),measurement_matric(2,:),'*g');
figure;imshow(frame51);hold on; title 'frame51';
plot(estimated_points_origin(51*2-1,:),estimated_points_origin(51*2,:),'.r');
plot(measurement_matric(51*2-1,:),measurement_matric(51*2,:),'*g');
figure;imshow(frame101);hold on;title 'frame101';
plot(estimated_points_origin(101*2-1,:),estimated_points_origin(101*2,:),'.r');
plot(measurement_matric(101*2-1,:),measurement_matric(101*2,:),'*g');
residual=measurement_matric_center-M*S;
residual=residual.^2;
distance=zeros(size(residual,1)/2,1);
for i =1:size(distance,1)
    distance(i)=sum(sqrt(residual(2*i-1,:)+residual(2*i,:)));
end
figure;title 'residuals per frame';
plot(distance);


